/*
 * To change this template, choose Tools | Templates
 * and open the template in the editor.
 */

package com.team1160.breakaway.drive;

import com.team1160.breakaway.api.Constants;
import edu.wpi.first.wpilibj.Encoder;
import edu.wpi.first.wpilibj.Jaguar;
import edu.wpi.first.wpilibj.RobotDrive;

/**
 *
 * @author FireBird Robotics
 */
public class Drive {


    RobotDrive roboDrive;
    
    //Static Instance Fields
    ////////////////////////////////////////////////////////////////////////////
    //assuming two drivetrain motors
    protected Jaguar m_left_front, m_left_back;
    protected Jaguar m_right_front, m_right_back;
    //Encoders
    protected Encoder e_left_drive;
    protected Encoder e_right_drive;

    //State Variables
    protected double ftPerSec;

    //constructors
    ////////////////////////////////////////////////////////////////////////////
    public Drive(){
        this.ftPerSec = 0;

        initMotors();
        initEncoders();
        initRobotDrive();
    }

    //Private Methods/Init

    private void initMotors(){
        //Motors Init
        this.m_left_back = new Jaguar(Constants.C_LEFT_DRIVE_BACK);
        this.m_left_front = new Jaguar(Constants.C_LEFT_DRIVE_FRONT);

        this.m_right_back = new Jaguar(Constants.C_RIGHT_DRIVE_BACK);
        this.m_right_front = new Jaguar(Constants.C_RIGHT_DRIVE_FRONT);

        //Set Bounds
        this.m_left_back.setBounds(Constants.JAGUAR_MAX_LIMIT,
                Constants.JAGUAR_DEADBAND_TOP,
                Constants.JAGUAR_CENTER,
                Constants.JAGUAR_DEADBAND_BOT,
                Constants.JAGUAR_MIN_LIMIT);
        this.m_left_front.setBounds(Constants.JAGUAR_MAX_LIMIT,
                Constants.JAGUAR_DEADBAND_TOP,
                Constants.JAGUAR_CENTER,
                Constants.JAGUAR_DEADBAND_BOT,
                Constants.JAGUAR_MIN_LIMIT);
        this.m_right_back.setBounds(Constants.JAGUAR_MAX_LIMIT,
                Constants.JAGUAR_DEADBAND_TOP,
                Constants.JAGUAR_CENTER,
                Constants.JAGUAR_DEADBAND_BOT,
                Constants.JAGUAR_MIN_LIMIT);
        this.m_right_front.setBounds(Constants.JAGUAR_MAX_LIMIT,
                Constants.JAGUAR_DEADBAND_TOP,
                Constants.JAGUAR_CENTER,
                Constants.JAGUAR_DEADBAND_BOT,
                Constants.JAGUAR_MIN_LIMIT);
    }

    private void initEncoders(){
        //Encoder Init
        this.e_left_drive = new Encoder(Constants.C_LEFT_DRIVE_ENCODER_A,
                Constants.C_LEFT_DRIVE_ENCODER_B);
        this.e_right_drive = new Encoder(Constants.C_RIGHT_DRIVE_ENCODER_A,
                Constants.C_RIGHT_DRIVE_ENCODER_B);
    }

    private void initRobotDrive()
    {
        this.roboDrive = new RobotDrive(this.m_left_front,
                                        this.m_left_back,
                                        this.m_right_front,
                                        this.m_right_back);
    }

    //Accessor methods
    ////////////////////////////////////////////////////////////////////////////
    /**
     * Gets the current speed of the robot in ftPerSec.
     *
     * @return Current ftPerSec
     */
    public double getSpeed(){
        return this.ftPerSec;
    }

    //action methods
    ////////////////////////////////////////////////////////////////////////////
    /**
     * This is a wrapper function for RobotDrive.tankDrive()
     * @param leftSpeed speed of the left motor
     * @param rightSpeed speed of the right motor
     */
    public void drive(double leftSpeed, double rightSpeed) throws IllegalArgumentException{
        
        if(!( (isInBounds(leftSpeed))&&(isInBounds(rightSpeed))))
            throw new IllegalArgumentException("One or more of the arguments " +
                                               "are not between 1.00 and -1.00");

        this.ftPerSec=0;
        //TODO change the values in these functions to ACTUAL VALUES
        this.roboDrive.tankDrive(leftSpeed, rightSpeed);
    }

    public void calcSpeed(){
        //TODO add calculation of speed here
    }
    //Testing Functions
    ////////////////////////////////////////////////////////////////////////////
    protected static boolean isInBounds(double value){
        if((value>=-1.00)&&(value<=1.00))
            return true;
        return false;
    }
}
